{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "from local_plot import *\n",
    "from utils import *\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [1, 2]\n",
    "output_folder = \"//home/xuhao/Dropbox/data/d2slam/two_drone_realworld_2023_1_14/swarm_log_drone1_2/swarm_log_latest/\"\n",
    "dte = 170\n",
    "paths_vo, t0 = read_paths(output_folder, nodes, t0=None, prefix=\"d2vins_\", dte=dte)\n",
    "paths_pgo, _ = read_paths(output_folder, nodes, t0=t0, prefix=\"pgo_\", dte=dte)\n",
    "paths_gt, _ = read_paths(output_folder, nodes, t0=t0, prefix=\"groundtruth_SwarmNode\", dte=dte, reset_orientation=False)\n",
    "align_paths(paths_vo, paths_gt, align_with_minize=False, align_coor_only=False, align_by_first=True)\n",
    "align_paths(paths_pgo, paths_gt, align_with_minize=False, align_coor_only=False, align_by_first=True)\n",
    "display(plot_fused_err(nodes, paths_vo, paths_gt, dte=dte, show=False))\n",
    "display(plot_relative_pose_err(1, nodes[1:], paths_vo, paths_gt, dte=dte, show=False))\n",
    "plot_fused(nodes, paths_vo, poses_pgo=paths_pgo, poses_gt=paths_gt, figsize=(10,10), output_path=output_folder)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [1, 2]\n",
    "output_folder = \"//home/xuhao/Dropbox/data/d2slam/two_drone_realworld_2023_1_14/swarm_log_drone1_3/swarm_log_latest/\"\n",
    "dte = 230\n",
    "paths_vo, t0 = read_paths(output_folder, nodes, t0=None, prefix=\"d2vins_\", dte=dte)\n",
    "paths_pgo, _ = read_paths(output_folder, nodes, t0=t0, prefix=\"pgo_\", dte=dte)\n",
    "paths_gt, _ = read_paths(output_folder, nodes, t0=t0, prefix=\"groundtruth_SwarmNode\", dte=dte, reset_orientation=False)\n",
    "align_paths(paths_vo, paths_gt, align_with_minize=True, align_coor_only=False, align_by_first=True)\n",
    "align_paths(paths_pgo, paths_gt, align_with_minize=True, align_coor_only=False, align_by_first=True)\n",
    "display(plot_fused_err(nodes, paths_vo, paths_gt, poses_pgo=paths_pgo, dte=dte, show=False))\n",
    "display(plot_relative_pose_err(1, nodes[1:], paths_vo, paths_gt, dte=dte, show=False))\n",
    "plot_fused(nodes, paths_vo, poses_pgo=paths_pgo, poses_gt=paths_gt, figsize=(10,10), output_path=output_folder)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [1, 2]\n",
    "output_folder = \"//home/xuhao/Dropbox/data/d2slam/two_drone_realworld_2023_1_14/swarm_log_drone1_4/swarm_log_latest/\"\n",
    "dte = 165\n",
    "paths_vo, t0 = read_paths(output_folder, nodes, t0=None, prefix=\"d2vins_\", dte=dte)\n",
    "paths_pgo, _ = read_paths(output_folder, nodes, t0=t0, prefix=\"pgo_\", dte=dte)\n",
    "paths_gt, _ = read_paths(output_folder, nodes, t0=t0, prefix=\"groundtruth_SwarmNode\", dte=dte, reset_orientation=False)\n",
    "align_paths(paths_vo, paths_gt, align_with_minize=True, align_coor_only=False, align_by_first=True)\n",
    "align_paths(paths_pgo, paths_gt, align_with_minize=True, align_coor_only=False, align_by_first=True)\n",
    "display(plot_fused_err(nodes, paths_vo, paths_gt, poses_pgo=paths_pgo, dte=dte, show=False))\n",
    "display(plot_relative_pose_err(1, nodes[1:], paths_vo, paths_gt, dte=dte, show=False))\n",
    "plot_fused(nodes, paths_vo, poses_gt=paths_gt, figsize=(10,10), output_path=output_folder)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [1, 2]\n",
    "output_folder = \"//home/xuhao/Dropbox/data/d2slam/two_drone_realworld_2023_1_14/swarm_log_drone1_5/swarm_log_latest/\"\n",
    "dte = 170\n",
    "paths_vo, t0 = read_paths(output_folder, nodes, t0=None, prefix=\"d2vins_\", dte=dte)\n",
    "paths_pgo, _ = read_paths(output_folder, nodes, t0=t0, prefix=\"pgo_\", dte=dte)\n",
    "paths_gt, _ = read_paths(output_folder, nodes, t0=t0, prefix=\"groundtruth_SwarmNode\", dte=dte, reset_orientation=False)\n",
    "align_paths(paths_vo, paths_gt, align_with_minize=False, align_coor_only=False, align_by_first=True)\n",
    "align_paths(paths_pgo, paths_gt, align_with_minize=False, align_coor_only=False, align_by_first=True)\n",
    "display(plot_fused_err(nodes, paths_vo, paths_gt, poses_pgo=paths_pgo, dte=dte, show=False))\n",
    "display(plot_relative_pose_err(1, nodes[1:], paths_vo, paths_gt, dte=dte, show=False))\n",
    "plot_fused(nodes, paths_vo, poses_pgo=paths_pgo, poses_gt=paths_gt, figsize=(10,10), output_path=output_folder)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "paths_pgo"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5 (default, Sep  4 2020, 07:30:14) \n[GCC 7.3.0]"
  },
  "vscode": {
   "interpreter": {
    "hash": "08ce52785f0fedc81003ce387e097a83d6cc9494681cd746006386992005bb71"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
